#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Hubs,  S2, HTMotor,  HTMotor,  HTMotor,  none)
#pragma config(Sensor, S3,     MUX,            sensorI2CCustomFastSkipStates)
#pragma config(Sensor, S4,     ProtoBoard,     sensorI2CCustomFastSkipStates9V)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop, reversed, encoder)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop, reversed, encoder)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     RightBack,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     LeftBack,      tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S2_C1_1,     RightFront,    tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S2_C1_2,     LeftFront,     tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S2_C2_1,     Lift1,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_2,     Lift2,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C3_1,     Lift3,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C3_2,     Lift4,         tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    LeftGripper1,         tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    RightGripper1,        tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    LeftGripper2,         tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    RightGripper2,        tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    Forward,              tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C2_6,    Side,                 tServoContinuousRotation)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.

#include "SmuxConfig.h"
#include "drivers/hitechnic-eopd.h"
#include "LiftManager.h"						//Automatic lift positioner
#include "3785_motion_functions.h"	//Motion fuctions such as forwards and backwards
#include "IRController.h"						//Include more precise autonomous IR reading function

void frontGripperAdjust(int position)
{
	servo[LeftGripper1]=255-position;
	servo[RightGripper1]=position;
}
void backGripperAdjust(int position)
{
	servo[LeftGripper2]=255-position;
	servo[RightGripper2]=position;
}

void initializeRobot()
{
	servoChangeRate[LeftGripper1]=5;
	servoChangeRate[LeftGripper2]=5;
	servoChangeRate[RightGripper1]=5;
	servoChangeRate[RightGripper2]=5;
	// Place code here to sinitialize servos to starting positions.
	// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
	frontGripper.open=false;
	backGripper.open=false;
	frontGripperAdjust(128);
	backGripperAdjust(128);
	HTEOPDsetShortRange(EOPD);
	return;
}

task main()
{
	initializeRobot();

	waitForStart();   // wait for start of tele-op phase

	while(

}
